// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float64.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "cansend.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <iostream>
#include <cmath>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h> // 新增头文件用于设置非阻塞

using namespace std::chrono_literals;
BYTE get_data[2] = {0x01, 0x00};
int encoder_id = 0x0000;
int joint_1data;
float joint_4data;

// 台号1
BYTE OPEN_1_1[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X01};
BYTE OPEN_1_2[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X02, 0X01};
BYTE OPEN_1_3[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X03, 0X01};
BYTE OPEN_1_4[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X04, 0X01};
BYTE CLOSE_1[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};
// 台号2
BYTE OPEN_2_1[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X01};
BYTE OPEN_2_2[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X02, 0X01};
BYTE OPEN_2_3[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X03, 0X01};
BYTE OPEN_2_4[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X04, 0X01};
BYTE CLOSE_2[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};

// 多线程
int ret;
int m_run0 = 1;
pthread_t threadid;

// 3.自定义节点类；
class Arm_Encoder : public rclcpp::Node
{
public:
    Arm_Encoder(int can_socket) : Node("arm_encoder_node_cpp"), can_socket_(can_socket)
    {

        // 关键修改1：设置socket为非阻塞模式
        int flags = fcntl(can_socket_, F_GETFL, 0);
        fcntl(can_socket_, F_SETFL, flags | O_NONBLOCK);
        std::cout << "已发送" << std::endl;

        SendData(can_socket, encoder_id, false, get_data, 2);

        joint1_pub = this->create_publisher<std_msgs::msg::Int32>("joint1_p", 10);
        joint4_pub = this->create_publisher<std_msgs::msg::Int32>("joint4_p", 10);


        ret = pthread_create(&threadid, NULL, &Arm_Encoder::receive_func, this);
    }

    ~Arm_Encoder()
    {
        // 关键修改2：添加线程终止机制
        pthread_cancel(threadid); // 强制终止线程
        pthread_join(threadid, NULL);
    }

    void ReceiveData()
    {
        float angle_joint1;
        float angle_joint4;

        struct can_frame frame;
        struct sockaddr_can addr;
        socklen_t len = sizeof(addr);
        auto joint1_p = std_msgs::msg::Int32();
        auto joint4_p = std_msgs::msg::Int32();

        while (rclcpp::ok()) // 关键修改3：添加退出条件检查
        {
            int nbytes = recvfrom(can_socket_, &frame, sizeof(frame), 0,
                                  (struct sockaddr *)&addr, &len);
            if (nbytes < 0)
            {
                if (errno == EAGAIN)
                { // 非阻塞模式无数据
                    std::this_thread::sleep_for(10ms);
                    continue;
                }
                break;
            }

            // 原有数据处理逻辑保持不变
            if (frame.can_id == 0x181)
            {
                // std::cout << "RX ID:0x" << std::hex << frame.can_id << std::endl;
                for (int i = 0; i < frame.can_dlc; i++)
                {
                    uint8_t receiveData[4];
                    // std::cout << " data:" << std::hex << (int)frame.data[i];
                    receiveData[i] = (int)frame.data[i];
                    uint8_t changeData[3];
                    uint32_t hexNumber = 0;
                    int32_t encoder2_data;
                    changeData[0] = receiveData[2];
                    changeData[1] = receiveData[1];
                    changeData[2] = receiveData[0];
                    // printf("\n");

                    // 将data中的三个字节组合成一个32位的十六进制数
                    hexNumber |= changeData[0]; // 将第一个字节赋值给hexNumber的最高8位
                    hexNumber <<= 8;            // 左移8位，为下一个字节腾出空间
                    hexNumber |= changeData[1]; // 将第二个字节赋值给hexNumber的次高8位
                    hexNumber <<= 8;            // 左移8位，为下一个字节腾出空间
                    hexNumber |= changeData[2]; // 将第三个字节赋值给hexNumber的最低8位

                    // printf("hexNumber: 0x%06X\n", hexNumber);
                    // 检查符号位
                    if (hexNumber & 0x800000)
                    {
                        // 如果符号位为1，表示负数
                        // 将补码转换为正数的补码表示
                        // uint32_t positiveComplement = ~hexNumber + 1;
                        // // 将得到的绝对值取负
                        // int32_t negativeValue = -((int32_t)positiveComplement);
                        hexNumber |= 0xFF000000; // 扩展符号位到32位
                        encoder2_data = hexNumber;
                        // int32_t negativeValue = (int32_t)hexNumber; // 直接转换为有符号
                        // encoder2_data = negativeValue;
                    }
                    else
                    {
                        encoder2_data = hexNumber;
                        // 如果符号位为0，表示正数
                    }
                    angle_joint1 = ((float)encoder2_data) * 360 / 16384;
                    // 需要四舍五入成整数，然后以Int形式发布
                    // joint_1data = round(angle_joint1 * 1.175);
                    joint_1data = round(angle_joint1 / (102 * 3.92));
                    // joint_1data = 5;
                    // printf("角度是：%d \n", joint_1data);
                    joint1_p.data = joint_1data;
                    // joint1_pub->publish(joint1_p);
                }
            }
            else if (frame.can_id == 0x182)
            {
                // std::cout << "RX ID:0x" << std::hex << frame.can_id << std::endl;
                for (int i = 0; i < frame.can_dlc; i++)
                {
                    uint8_t receiveData[4];
                    // std::cout << " data:" << std::hex << (int)frame.data[i];
                    receiveData[i] = (int)frame.data[i];
                    uint8_t changeData4[3];
                    uint32_t hexNumber4 = 0;
                    int32_t encoder4_data;
                    changeData4[0] = receiveData[2];
                    changeData4[1] = receiveData[1];
                    changeData4[2] = receiveData[0];
                    // printf("\n");

                    // 将data中的三个字节组合成一个32位的十六进制数
                    hexNumber4 |= changeData4[0]; // 将第一个字节赋值给hexNumber的最高8位
                    hexNumber4 <<= 8;             // 左移8位，为下一个字节腾出空间
                    hexNumber4 |= changeData4[1]; // 将第二个字节赋值给hexNumber的次高8位
                    hexNumber4 <<= 8;             // 左移8位，为下一个字节腾出空间
                    hexNumber4 |= changeData4[2]; // 将第三个字节赋值给hexNumber的最低8位

                    // printf("hexNumber4: 0x%06X\n", hexNumber4);
                    // 检查符号位
                    if (hexNumber4 & 0x800000)
                    {
                        // 如果符号位为1，表示负数
                        // 将补码转换为正数的补码表示
                        hexNumber4 |= 0xFF000000; // 扩展符号位到32位
                        encoder4_data = hexNumber4;
                    }
                    else
                    {
                        encoder4_data = hexNumber4;
                        // 如果符号位为0，表示正数
                    }
                    angle_joint4 = ((float)encoder4_data) * 360 / 16384;
                    // 需要四舍五入成整数，然后以Int形式发布
                    // 此处需根据实际情况调整
                    joint_4data = round(angle_joint4 * 1.232);
                    // printf("角度是：%f \n", angle_joint4);
                    joint4_p.data = joint_4data;
                    // joint4_pub->publish(joint4_p);
                }
            }
            printf("1角度是：%d \n", joint_1data);
            joint1_pub->publish(joint1_p);
            printf("4角度是：%f \n", joint_4data);
            joint4_pub->publish(joint4_p);
        }
    }

private:
    int can_socket_; // 关键修改4：将socket存储为成员变量
    int state_first = 0;
    int state_second = 0;
 
    std::mutex can_mutex;

    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr joint1_pub;
    rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr joint4_pub;
  

    static void *receive_func(void *param)
    {
        Arm_Encoder *encoder = static_cast<Arm_Encoder *>(param);
        encoder->ReceiveData();
        return nullptr;
    }

    void valveCallback(const std_msgs::msg::Int32MultiArray::SharedPtr state_msg)
    {
        // can卡序号 canid 是由扩展帧 数据 数据长度
        state_first = state_msg->data[0];
        state_second = state_msg->data[1];
        std::cout << state_first << std::endl;
        std::cout << state_second << std::endl;

        switch (state_first)
        {

        case 0:
            SendData(can_socket_, 0X01, false, CLOSE_1, 8);
            break;
        case 1:
            SendData(can_socket_, 0X01, false, OPEN_1_1, 8);
            break;
        case 2:
            SendData(can_socket_, 0X01, false, OPEN_1_2, 8);
            break;
        case 3:
            SendData(can_socket_, 0X01, false, OPEN_1_3, 8);
            break;
        case 4:
            SendData(can_socket_, 0X01, false, OPEN_1_4, 8);
            break;
        }

        switch (state_second)
        {

        case 0:
            SendData(can_socket_, 0X01, false, CLOSE_2, 8);
            break;
        case 1:
            SendData(can_socket_, 0X01, false, OPEN_2_1, 8);
            break;
        case 2:
            SendData(can_socket_, 0X01, false, OPEN_2_2, 8);
            break;
        case 3:
            SendData(can_socket_, 0X01, false, OPEN_2_3, 8);
            break;
        case 4:
            SendData(can_socket_, 0X01, false, OPEN_2_4, 8);
            break;
        }
    }
};

int main(int argc, char const *argv[])
{
    rclcpp::init(argc, argv);
    int can1 = init_can("can1");
    // 关键修改5：传递socket到节点
    // rclcpp::spin(std::make_shared<Arm_Encoder>(can1));
    auto node = std::make_shared<Arm_Encoder>(can1);
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    close(can1);
    return 0;
}